A Pseudospectral Method for Real-Time Motion Planning and Obstacle Avoidance

In recent years, pseudospectral (PS) methods have been successfully applied to solve a rich number of trajectory optimization problems arising in aerospace application. Motivated by the success of this approach, we consider the problem of generating minimum-time trajectories for unmanned ground vehicles. Shapes of arbitrary number, size and configuration are modelled in the form of path constraints in the resulting optimal control problem formulation. PS methods are used to solve the constrained, nonlinear optimal control problem. Solutions are obtained within a few seconds even under a MATLAB environment running on legacy computer hardware. A complete problem formulation is developed including a derivation of the necessary conditions for optimality. The trajectories are mathematical extremals as they satisfy all the optimality conditions. What is noteworthy about our approach is that the difficulties associated in solving the Hamiltonian equations are completely circumvented by an application of the Covector Mapping Theorem. Solved examples are provide to illustrate the tools and techniques. 1.0 INTRODUCTION Trajectory planning occurs within the guidance system of every autonomous or semi-autonomous vehicle, whether it is the $300 Roomba® or the $300M New Horizons spacecraft. Trajectory planning allows the vehicle to travel from one location to another safely, within the limits of its electrical and mechanical capabilities, and in some situations, in an optimal manner with respect to use of fuel, expenditure of time, or distance travelled. The development of autonomous trajectory planning technologies has been painstakingly slow; the variety of tasks accomplished by every five-year-old human cannot, at this point in time, be replicated by a multi-million dollar robot. The advantages, however, to accomplishing human-like tasking with a robot are immeasurable and for that reason continue to be a goal of researchers everywhere. 1.1 Motivation A broad class of problems are emerging that implore the ability to remove the human from the control loop. The rewards of robotic systems are increased production, reliability, safety, and capability. Spacecraft assembly and repair, space station replenishment, and certain facets of planetary exploration were viewed solely as human missions during the Apollo and early Space Shuttle periods. Ever improving technologies and capabilities and cost and risk reduction have been the driving forces in reshaping these paradigms; now, systems are either being developed or have already been fielded to accomplish these tasks unmanned. The Lewis, L.R.; Ross, I.M. (2007) A Pseudospectral Method for Real-Time Motion Planning and Obstacle Avoidance. In Platform Innovations and System Integration for Unmanned Air, Land and Sea Vehicles (AVT-SCI Joint Symposium) (pp. 10-1 – 10-22). Meeting Proceedings RTO-MP-AVT-146, Paper 10. Neuilly-sur-Seine, France: RTO. Available from: http://www.rto.nato.int/abstracts.asp. Report Documentation Page Form Approved OMB No. 0704-0188 Public reporting burden for the collection of information is estimated to average 1 hour per response, including the time for reviewing instructions, searching existing data sources, gathering and maintaining the data needed, and completing and reviewing the collection of information. Send comments regarding this burden estimate or any other aspect of this collection of information, including suggestions for reducing this burden, to Washington Headquarters Services, Directorate for Information Operations and Reports, 1215 Jefferson Davis Highway, Suite 1204, Arlington VA 22202-4302. Respondents should be aware that notwithstanding any other provision of law, no person shall be subject to a penalty for failing to comply with a collection of information if it does not display a currently valid OMB control number. 1. REPORT DATE 01 NOV 2007 2. REPORT TYPE N/A 3. DATES COVERED 4. TITLE AND SUBTITLE A Pseudospectral Method for Real-Time Motion Planning and Obstacle Avoidance 5a. CONTRACT NUMBER 5b. GRANT NUMBER 5c. PROGRAM ELEMENT NUMBER 6. AUTHOR(S) 5d. PROJECT NUMBER 5e. TASK NUMBER 5f. WORK UNIT NUMBER 7. PERFORMING ORGANIZATION NAME(S) AND ADDRESS(ES) Department of Mechanical and Astronautical Engineering Naval Postgraduate School, Monterey, CA 93943 USA 8. PERFORMING ORGANIZATION REPORT NUMBER 9. SPONSORING/MONITORING AGENCY NAME(S) AND ADDRESS(ES) 10. SPONSOR/MONITOR’S ACRONYM(S) 11. SPONSOR/MONITOR’S REPORT NUMBER(S) 12. DISTRIBUTION/AVAILABILITY STATEMENT Approved for public release, distribution unlimited 13. SUPPLEMENTARY NOTES See also ADM202416., The original document contains color images. 14. ABSTRACT 15. SUBJECT TERMS 16. SECURITY CLASSIFICATION OF: 17. LIMITATION OF ABSTRACT UU 18. NUMBER OF PAGES 22 19a. NAME OF RESPONSIBLE PERSON a. REPORT unclassified b. ABSTRACT unclassified c. THIS PAGE unclassified Standard Form 298 (Rev. 8-98) Prescribed by ANSI Std Z39-18 A Pseudospectral Method for Real-Time Motion Planning and Obstacle Avoidance 10 2 RTO-MP-AVT-146 UNCLASSIFIED/UNLIMITED UNCLASSIFIED/UNLIMITED Mars rovers Spirit and Opportunity are shining examples of the utilization of robotic technologies to aid human exploration, accomplishing tasks that could not be achieved otherwise. The applications of trajectory planning are not limited to systems outside the Earth’s atmosphere. Military applications of unmanned and autonomous vehicles have drawn considerable interest and recognition in recent years. Most notable is the Unmanned Aerial Vehicle (UAV), but considerable advances are making the incorporation of unmanned ground (UGV) and sea surface vehicles possible. Meanwhile, new classes of UAVs are under development that are carried by soldiers on the ground, deployed only when operationally needed, and implemented without the overhead of the larger current systems. These systems remove humans from jobs that would be otherwise extremely dangerous. Moving away from the public sector, the private arena is the ultimate stage for technological utility. While the future possibilities are truly endless, robotic technologies are already improving consumer lives by eliminating such tedious tasks as vacuuming and parallel parking. 1.2 Background The various applications of trajectory planning led to the evolution of differing techniques to solve their problems. Aeronautical and space applications drove the development of nonlinear optimal control techniques and continue to do so [1-6]. In these situations, global problem knowledge is assumed, vehicle motion is precisely understood, few physical constraints obstruct the vehicular motion, and trajectory optimization is key to design and mission success. Robotics applications propelled a differing solution, but it was a solution better adapted to fit the inherent situational complexities including: limited problem knowledge, noisy sensors, uncertain dynamical characteristics, intricate obstacle-rich environments, limited computational power, and the necessity to generate feasible solutions. Despite the general similarity between problems, these two fields of application experienced little cross-pollinization. Aerospace applications desired optimality and robotics applications desired simplicity and an ability to handle uncertainty. Many techniques fall within the realm of robotics-based trajectory planning [7-8]. Potential functions were first developed over thirty years ago [9], and though they can be simplistic, they are still used in many functions today [10-11]. In general, this approach can be used successfully in the presence of uncertainty but it suffers from occurrences of local minima, a lack of trajectory optimality, and difficulty in accommodating complex vehicle constraints [12]. More recent developments place heavy emphasis on sampling-based planning techniques. Notably, sampling methods include Probabilistic Road Maps (PRM), Rapidly-Exploring Random Trees (RRT), and Expansive Space Trees (EST) to name a few [8, 13-15]. On the whole, these methods use a probabilistic means of connecting the initial configuration to the final configuration, enabling an improved capacity to handle uncertainty, a relative blindness to environmental complexity, and a capability to generate feasible solutions with minimal computational burden. Complexity increases with the incorporation of planning techniques to satisfy dynamic constraints and improve solution optimality; however, solution optimality is never guaranteed [15]. Nonlinear optimal control methods offer significant advantages to other approaches, namely the inherent satisfaction of vehicle and problem constraints and the intrinsic extremal nature of solutions. Generally, two distinct methods exist for solving the nonlinear optimal control problems [16-17], direct and indirect. Indirect methods make use of Pontryagin’s maximum principle and create a two-point boundary value problem. Direct methods transfer the problem into a nonlinear programming problem by discretizing the trajectory and this problem can be solved by any one of a number of nonlinear optimization solvers. Typically direct methods are considered more numerically advantageous that indirect methods, but despite their differences, they all share the same concerns including: solution convergence, computational complexity, and the need for a good initial guess. Advances in computing power and improvement in optimization algorithms are changing that paradigm and research is showing the applicability of optimal control techniques in guidance and control A Pseudospectral Method for Real-Time Motion Planning and Obstacle Avoidance RTO-MP-AVT-146 10 3 UNCLASSIFIED/UNLIMITED UNCLASSIFIED/UNLIMITED [18-21]. Whereas optimization methods traditionally created a solution in minutes to hours, solutions can now be found within seconds or less, and the improvements in computational speed increase their ability to handle problem uncertainty. Aerospace applications made use of optimal control solutions because the problems typically permitted the trajectories to be generated offline; now the improvements in optimal control methods present a strong argument for their application in online planners and with robotics problems. 2.0 SOLUTION APPROACH Given the progress made in applying optimal control methods to online aerospace applications [18-19] and being aware of the ability to eliminate the associated computational burden [20, 22], this paper focuses on the application of these techniques to a new breed of problems, sharing attributes with both aerospace and robotics applications. This problem is that of the unmanned vehicle, characterized by a lack of global knowledge, complex obstacle-rich environments, and a need for feasible solutions in the face of uncertainty. On the other hand, these systems typically incorporate sophisticated sensors and ample computational power. The research presented here evaluates and validates the concept of trajectory planning for unmanned vehicles with optimal control methods. Being a feasibility study, robustness is not proven, and while the concerns of uncertainty are currently being evaluated, those efforts are not discussed here. Instead, the primary thrust of this work is the optimization of kinematic trajectories within varying, complex environments. The optimization tool used during this exercise was DIDO, a software package based on pseudospectral methods [1-6] that runs within the MATLAB environment. 3.0 THE OPTIMAL CONTROL PROBLEM One of the many advantages to utilizing optimal control methods is the relative ease with which a multitude of problems is formulated. Any kinematic, dynamic, or path constraint or optimality criteria that is appropriately expressed in a mathematical manner can be rapidly incorporated into the problem formulation. This inherent portability and flexibility makes these techniques powerful, but the fundamental key is to express the characteristics appropriately. In this section, the UGV problem will be defined within the framework of an optimal control problem; all characteristics will be covered including the kinematic equations, path constraints, scaling, and necessary conditions. Even though the detailed focus remains on one version of an UGV, the specific equations are not as important as the validation of the method; due to generality of this method any system could be represented. 3.1 Unmanned Ground Vehicle The term “unmanned ground vehicle” is generic; there are a multitude of systems that could fit that name. In an attempt to maintain generality, a common example is used – a four-wheeled car with rear-wheel drive and front-wheel steering. This system is well-studied, and the nonholonomic-nature of the constraints adds kinematic complexity. A simple, kinematic model of the car [8] is shown in Figure 1 and the state and control vectors are presented in Equations (1) and (2), respectively. The x-y location of the car represents the current position of the center point of the rear axle. The car’s orientation angle is measured with respect to the horizontal axis and is presented as the state variable theta. The steering angle, phi, is measured with respect to the car’s heading, or velocity vector, and the variable ‘L’ measures the distance between the front and rear axles. A Pseudospectral Method for Real-Time Motion Planning and Obstacle Avoidance 10 4 RTO-MP-AVT-146 UNCLASSIFIED/UNLIMITED UNCLASSIFIED/UNLIMITED